using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using RobotSimulator.Basic;
using Unity.VisualScripting;
public class WheelLegRobotController:RobotController
{
    private WheelLegRobot robot;
    public Pid x_angle_pid;
    public Pid r_angle_pid;
    public float target_x=0;
    public float target_r = 0;
    protected override void Start()
    {
        base.Start();
        robot = (WheelLegRobot)control_target;
    }
    private CmdGetIMUData cmd=new CmdGetIMUData();
    private FixAngle fixAngle=new FixAngle();
    [HideInInspector]
    public IMUData data;
    public override void Control()
    {
        robot.imu.RecvCmd(cmd);
        if (robot.imu.output_data != "")
        {
            data=JsonUtility.FromJson<IMUData>(robot.imu.output_data);
            Vector3 angle = IMU.Euler(data.rotation);
            //Debug.Log(angle);
            float angle_y = fixAngle.Fix(angle.y);
            //Debug.Log(angle_y);
            float x_control_val= x_angle_pid.Control((target_x - angle.x)/90f);
            float r_control_val = r_angle_pid.Control((target_r - angle_y) / 90f);
            if (r_control_val > 3)
            {
                r_control_val = 3;
            }
            else if (r_control_val < -3)
            {
                r_control_val = -3;
            }
            robot.LeftWheel.speed = x_control_val+r_control_val;
            robot.RightWheel.speed = x_control_val-r_control_val;

        }
       
    }
}
